#include "hw3.h"

#define pixle(X,i,j) (unsigned char)(X->imageData + (i)*X->widthStep)[(j)*X->nChannels]
#define pixleSet(X,i,j) (X->imageData + (i)*X->widthStep)[(j)*X->nChannels]

int display; 

char *depthImageFileName;
char *confImageFileName;
char *grayImageFileName;

char *leftInputImageFileName;
char *rightInputImageFileName;

int parseCommandLineArgs(int argc, char** argv)
{

	static struct option long_options[] =
	{
		//{name, has_arg, int *flag, int val}
		{"depth", 1, 0, 0},
		{"conf", 1, 0, 0},
		{0,0,0,0}
	};

	display = 0;
	depthImageFileName = "\0";
	confImageFileName = "\0";
	grayImageFileName = "\0";
	leftInputImageFileName = "\0";
	rightInputImageFileName = "\0";

	int option_index = 0;
	int c;
	while (1) {
		c = getopt_long (argc, argv, "do:",
		       long_options, &option_index);

		if (c == -1)
			break;

		switch (c) {
			case 'd':
				display = 1;
			break;
			case 'o':
				grayImageFileName = optarg;
			break;
			break;
			case '?':
	  			/* getopt_long already printed an error message. */
	  		break;
			// long only options
			case 0:
			switch (option_index) {
				case 0:
					depthImageFileName = optarg; 
				break;
				case 1:
					confImageFileName = optarg;
				break;
			}
			break;
		}
	}

	if (optind < argc) {
		// the unspecified option is the input file name
		leftInputImageFileName = argv[optind++];
		rightInputImageFileName = argv[optind];
	}
	return 1;
}

	#define kernal_size 9

unsigned int SSD(char kernal[kernal_size][kernal_size], IplImage* rGrayImg, int start_row, int start_col)
{
	
	unsigned int sum = 0;
	for (int j = 0; j < kernal_size; j++) {
		for (int k = 0; k < kernal_size; k++) {
			int diff = pixle(rGrayImg, start_row+j, start_col + k) - kernal[j][k];
			sum += diff * diff;
		}
	}

	return sum;
}

void calculateCorrespondence(IplImage* lGrayImg, IplImage* rGrayImg, IplImage* depthImg, int *disp, float *conf)
{

	char kernal[kernal_size][kernal_size];
	unsigned int *disparityLine = (unsigned int*) malloc(sizeof(int) * lGrayImg->width);

	int minDisp = 10000;
	int maxDisp = 0;

	// go down the rows
	for (int i = kernal_size-1; i < lGrayImg->height; i++) {
		
		// fill the kernal with the data for the begining of the line
		for (int j = 0; j < kernal_size; j++) {
			for (int k = 1; k < kernal_size; k++) {
				kernal[j][k] = pixle(lGrayImg, i - kernal_size + j + 1, k);
				//printf("%d,%d\n", i - kernal_size + j + 1, k);
			}
		}


		// go across the columns
		for (int j = kernal_size-1; j < lGrayImg->width; j++) {

			for (int k = 1; k < kernal_size; k++){
				for (int k2 = 0; k2 < kernal_size; k2++){
					kernal[k2][k-1] = kernal[k2][k];
				}
			}

			for (int k = 0; k < kernal_size; k++){
				kernal[k][kernal_size - 1] = pixle(lGrayImg, i - kernal_size + k + 1, j);
			}
/*	
			if (j < kernal_size + 3) {
				for (int k = 0; k < kernal_size; k++) {
					for (int k2 = 0; k2 < kernal_size; k2++) {
						printf("%3u ", (unsigned char)kernal[k][k2]);
					}
					printf("\n");
				}
			}
			if (j < kernal_size + 3)
				printf("\n");
*/
			unsigned int max = 0;
			unsigned int min = 100000000;
			unsigned int avg = 0;
			int minK;
			for (int k = 0; k < lGrayImg->width - kernal_size; k++) {
				disparityLine[k] = SSD(kernal, rGrayImg, i + 1 - kernal_size, k);

				if (disparityLine[k] < min) {
					min = disparityLine[k];
					minK = k;
				}
			}
			//printf("%d	:%d\n", min, minK);	
			disp[i*lGrayImg->width + j] = j - (kernal_size/2) - minK;
			if (disp[i*lGrayImg->width + j] > maxDisp)
				maxDisp = disp[i*lGrayImg->width + j];

			if (disp[i*lGrayImg->width + j] < minDisp)
				minDisp = disp[i*lGrayImg->width + j];
			//printf("%d	:%d,%d\n", disp[i*lGrayImg->width + j], minDisp, maxDisp);

		}
//		printf("\n");
	}

	maxDisp -= minDisp;

	for (int i = 0; i < lGrayImg->height; i++) {
		for (int j = 0; j < lGrayImg->width; j++) {
			pixleSet(depthImg, i, j) = (unsigned char)((disp[i*lGrayImg->width + j] - minDisp) * 255 / maxDisp);
			printf("%u\n", (unsigned char)pixle(depthImg,  i ,j));
		}
	}

}


int main(int argc, char *argv[])
{
	
	parseCommandLineArgs(argc, argv);
	
	IplImage* lImg = cvLoadImage(leftInputImageFileName);
	IplImage* rImg = cvLoadImage(rightInputImageFileName);

	if (!lImg || !rImg) {
		printf("Error loading input images\n");
		return 0;
	}


	// convert the input images to grayscale images
	IplImage* lGrayImg = cvCreateImage(cvSize(lImg->width,lImg->height),lImg->depth,1);
	IplImage* rGrayImg = cvCreateImage(cvSize(rImg->width,rImg->height),rImg->depth,1);
	IplImage* depthImg = cvCreateImage(cvSize(rImg->width,rImg->height),rImg->depth,1);
	cvCvtColor(lImg,lGrayImg,CV_BGR2GRAY);
	cvCvtColor(rImg,rGrayImg,CV_BGR2GRAY);

	// create arrays for the disparity map and confidence map
	int *disp = (int*) malloc(sizeof(float) * lImg->width * lImg->height);
	float *conf = (float*) malloc(sizeof(float) * lImg->width * lImg->height);
	
	calculateCorrespondence(lGrayImg, rGrayImg, depthImg, disp, conf);

	cvNamedWindow("Depth Image", CV_WINDOW_AUTOSIZE);
	cvShowImage("Depth Image", depthImg);

	cvWaitKey();

	return 0;
}

